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Appendix A 




Source Code Listing 



% ROGPS - Solves for user position and time based on ephemeris equations 
% and on time-difference measurements from 5 GPS satellites. 

% U (3x1) the users position 
% time (lxl) seconds 

% Ub (3x1) an initial guess at the users position 

% dU (3x1) is the adjustment made on the initial guess 

% sat (3x5) is the location of each satellite 

% H (4x4) matrix differentiating r_diff wrt (U.t) 

% satO is the (3x5) matrix of 5 sat positions at beginning of 
% interpolation interval. 

% sat2 is the (3x5) matrix of 5 sat positions at end of interpolation interval. 
% V is a (3x5) matrix of velocities for all 5 satellites. 

dU = 0; 

% step 1: Guess an initial Ubar, and time. 

U_true = [1300748.12M500694.5;4313770.5]; % Actual user location 
Ub = [1200748. 12;-^400694.5;43 19770.5]; % User location guess 

time_tme = 483738.75; 
time = 483638.75; 

interval = 300; % Linear interpolation (half) interval for sals. 
% select five satellites 

% Files that contain ephemeris constants. These happened to be 

% visible at the data collection time. 

svl = sv7_eph; . 

sv2 = sv2_eph ; 

sv3 = sv9_eph ; 

sv4 = sv4_eph ; 

sv5 = svl2_eph; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
% 

% Synthesize measurement vector 
% 
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% For now, ignore thai satellites move during signal propagation 

% time. Thus, set ephemeris time to measurement time, time_true. 

svl(l) = time_true; 

sv2(l) = time_true; 

S v3(l) = time_true; 

sv4(l) = time_true; 

sv5(l) = time_true; 

% Get satellite positions 
sat(:,l) = satellite_ephemeris(svl); 
sat(:,2) = satellite_ephemeris(sv2); 
sat(:,3) = satellite_ephemeris(sv3); 
sat(:,4) = satellite_ephemeris(sv4); 
sat(:,5) = satellite_ephemeris(sv5); 

% Calculate perfect measured range differences. 
fori= 1:4 

r_diff_t(i) = norm(sat(:,l) - U.true) - norm(sat(:,i+l) - U.true) ; 
end 

% Add noise to the measurements 
light_speed = 3e8 ; % Meters/sec 

dt_sigma = le-7 ; % One tenth of a chip. 
% r_diff_m = r_diff_t + (light.speed * dt_sigma * randn(l ,4)) ; 
r_diff_m = r_diff_t +[00010]; 
r_diff_m = r_diff_m* ; 

dr_var = dt_sigma * 3e8 * dt_sigma * 3e8 ; 

r_diff_covar = [ dr.var 0 0 0;... 

0 dr_var 0 0;... 

0 0 dr_var 0 ; ... 

0 0 0 dr_var] 

% 

% Find satellite average velocities 
% 

%%%%%%%%%%%%%%%%%**%%*************** 

% Set time to beginning of interpolation interval 
% First element of svi is always time. 
svl(l) = time - interval; 
sv2(l) = time - interval; 
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sv3(l) = time - interval; 
S v4(l) = time - interval; 
S v5(l) = time - interval; 

% Get satellite positions 
satA(:,l) = satellite_ephemeris(svl); 
satA(:.2) = satellite_ephemeris(sv2); 
satA(:,3) = satellite_ephemeris(sv3); 
satA(:,4) = satellite_ephemeris(sv4); 
satA(:,5) = sateilite_ephemeris(sv5); 

% Set time to end of interpolation interval. 
svl(l) = time + interval; 
sv2(l) = time + interval; 
sv3(l) = time + interval; 
sv4(l) = time + interval; 
sv5(l) = time + interval; 

% Get satellite positions. 
satB(:,l) = satellite_ephemeris(svl); 
satB(:,2) = satellite_ephemeris(sv2); 
satB(:,3) = satellite_ephemeris(sv3); 
satB(:,4) = satellite_ephemeris(sv4); 
satB(:^) = satellite_ephemeris(sv5); 

% calculate x,y,z velocities (in m/s) 
V = (satB-satA)/interval; 

% 

% Form initial H matrix of partials 
% 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 

% Partials taken at guessed time and user position. 
% Need partials of sat coordinates w.r.t time. Over the "interval" 
% specified above we assume sat velocity is constant. Set partials 
% to sat velocities. Compute vector and range from object to each 
% satellite. 

% Get satellite positions at guessed time. 
svl(l) = time ; 
sv2(l) = time ; 
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sv3(l) = time ; 
S v4(l) = time ; 
sv5(l) ss time ; 

sat(:,l) = satellite_ephemeris(svl); 
sat(:,2) = satellite_ephemeris(sv2); 
sat(:,3) = satellite_ephemeris(sv3); 
sat(:,4) = satellite_ephemeris(sv4); 
sat(:,5) = satellite_ephemeris(sv5); 

% Compute estimated sat user vectors and ranges, 
for i= 1:5, 

satJJ(:4) = sat(:a)-Ub; 

range(i) = norm(sat_U(:,i)) ; 

DV(: j) = sat_U(:4) / range(i) ; 
end 

% Form H matrix of partials at estimated user location and lime: 

% i'th row of H is H = (pDli/px, pDli/py, pDli/pz, pDli/pt) 

% First three elements is just the difference between the normalized 

% direction vectors to the first and to the i*th satellites. 

% Last element is difference between 1st and i'th inner products 

% of direction vectors with satellite velocity vectors. 

fori= 1:4, 

H(i,l:3) = DV(:,i+iy - DV(:,1)' ; 

H(i,4) = DV(:,iy * V(:,l) - DV(:,i+l)* * V(:,i+1) ; 
end 

% now compute the first estimate of G 
% G = inv(H); 

loop = 0; epsilon = 1; %meter 
while ( loop < 20) 

loop = loop + 1; 

% step 3 

% Substitute t into ephemeris data and get positions at guessed time. 
% Here we use actual ephemeris equations to find sat positions, 
% but note that the time derivatives of sat position are taken from 
% the linear interpolation. 
svl(l) = time; 
sv2(l) = time ; 
sv3(l) = time ; 
sv4(l) = time ; 
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sv5(l) = time ; 
sai(:.l) = satellite_ephemeris(svl); 
sat(:,2) = saiellUe__ephemeris(sv2); 
sat(:,3) = satellite_ephemeris(sv3); 
sat(:,4) = sateliite_ephemeris(sv4); 
sat(:,5) « satelliie_ephemeris(sv5); 

satl(:Joop) = sat(:,l); 
sat2(:Joop) = sat(:,2); 
sat3(:Joop) = sat(:,3); 
sat4(:,loop) = sat(:,4); 
sat5(:,loop) = sat(:,5); 

% Compute vector and range from object to each satellite. 

% Needed for new H matrix. 

for i= 1:5, 
sat_U(:,i) = sat(:4)-Ub; 

range(i) = norm(sat_U(:,i)) ; 

DV(:4) = sat_U(:4) / range(i) ; 

end 

% compute r_diff using Ubar and t 
r_diff(l) = range(l) - range(2) ; 
r_diff(2) = range(l) - range(3) ; 
r_diff(3) = range(l) - range(4) ; 
r_diff(4) = range(l) - range(5) ; 

% step 4 

% compute range-difference error. 
r_diff_error(:,loop) = r_diff_m - r_diff ' ; 

% step 5 

% Form H matrix of partials: 

% i'th row of H is H = (pDli/px, pDli/py, pDli^z, pDli/pt) 

% First three elements is just the difference between the normalized 

% direction vectors to the first and to the i'th satellites. 

% Last element is difference between 1st and i'th inner products 

% of direction vectors with satellite velocity vectors. 

fori= 1:4, 

H(i,l:3) = DV(:,i+l)' - DV(:,1)* ; 

H(i,4) = DV(:,iy * V(:,l) - DV(:,i+l)' * V(:,i+1) ; 
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end 

H4(:Joop) = H(:,4); 
% step 6 

% compute G via 1 iteration 
% G = G*(2*eye(4)-H*G); 
G = inv(H) ; 
G4(loop,:) = G(4,:) ; 

% step 7 

% compute dU as G * r_diff_error(: ( Ioop) 
dU4 = G * r_difT_error(:,loop) ; 
dU = dU4(l:3); 
dt^dU4(4); 

% step 8 

% update position and time estimate Ub = Ub + dU, time = time + dt 
ans_vec(:,loop) = [Ub\time]' ; 
Ub = Ub + dU; 
time = time + dt; 

loop; 
end % while 

U_covar = G * r_diff_covar * G* ; 
U_sigma(l) = sqrt(U_covar(l,l)) ; 
U_sigma(2) = sqrt(U_covar(2,2)) ; 
U_sigma(3) = sqrt(U_covar(33)) ; 
U_sigma(4) = sqrt(U_covar(4,4)) ; 

% Plot x,y,z, or t convergence vs loop iteration number. 
% 1 -> x, 2 -> y, 3 -> z, 4 -> time. 

% I 
% I 
% v 

plot(ans_vec(l,:)) ; 
end 



function [ vec_satel I i te_ec f] = satellite_ephemeris(input_vector) 

% The following code for calculating satellite positions from ephemeris 
% data is taken from ICD-GPS-2000, table 20-IV. 
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t 

mj) 
delta_n 



= input_vector(l,l) ; 
= input_vector(2,l) ; 



= input_vector(3,l) ; 



e 



= input_vector(4,l) ; 



sqrt_a = 
OMEGA J) 
LO 

omega 

OMEGA_dot 
i_dot 



= input_vector(5,l) ; 



c_uc 



c_rs 
c_ic 
c_is 
t_oe 
iode 



c_us 



c_rc 



= input_vector(6,l) ; 
= input_vector(7,l) ; 
= input_vector(8,l) ; 
= input_vector(9,l) ; 
= input_vector(10,l) ; 
= input_vector(ll,l) ; 
= input_vector(12,l) ; 
= input_vector(13,l) ; 
= input_vector(14,l) ; 
= input_vector(15,l) ; 
= input_vector(16,l) ; 
= input_vector(17,l) ; 



= input_vector(18,l) ; 



% Earth mass times universal gravity constant (see Ref l f pp 34). 



if t_k> 302400 

t_k = t_k-604800 ; %correct time for EOW crossovers 
elseift_k< -302400 

t_k = t_k+604800 ; %correct time for EOW crossovers 

end 

% Corrected mean angular motion. 

n = n_0 + delta_n ; 

% Mean anomaly (false sat orbit angle in orbit plane, using mean motion). 
m_k = m_0 + n*t_k ; 



mu 



= 3.986005el4 ; 



% Earth rotation rate. 
OMEGA_e_dot 



= 7.2921 151467e-5; 



% Orbit ellipse semimajor axis length 
a = (sqrt_a) A 2 ; 




RD-23 ,530 





- A8 - 



% Eccentric anomaly (true sat orbit angle in orbit plane, from ellipse center). 
e_k = kepler(e,m_k,0.01, 10000) ; 



% Terms for finding true anomaly. 

cosf.k = (cos(e_k) - e)/(l - e*cos(e_k)) ; 

sinf.k = ((1 - e A 2) A 0.5)*sin(e_k)/(l - e*cos(e_k)) ; 



% True anomaly (true sat orbit angle in orbit plane, from earth center). 
f_k = atan2(sinf_k,cosf_k) ; 



% Argument of lattitude (sat orbit angle from ascending node point on equator) 
phi_k = f_k + omega ; 



sin2phik = sin(2*phi_k) ; 
cos2phik = cos(2*phi_k) ; 

% Argument of lattitude correction. 

delta_u Jc = c_us*sin2phik + c_uc*cos2phik ; 

% Radius correction. 

delta_r_k = c_rc*cos2phik + c_rs*sin2phik ; 

% Correction to inclination. 

delta_i_k = c_ic*cos2phik + c_is*sin2phik ; 

% Corrected argument of lattitude. 

u_k = phi_k + delta_u_k ; 

% Corrected radius. 

rjc = a*(l - e*cos(e_k)) + delta_r_k ; 
% Corrected inclination. 

i_k = i_0 + delta_i_k + i_dot*t_k ; 

% Final x-y position in orbital plane. 
x_k_prime = r_k*cos(u_k) ; 
y_k_prime = r_k*sin(u_k) ; 

% Longitude of ascending node. 

omegajc = OMEG A_0 + (O ME G A_do t-O MEG A_e_do t) * t_k - OMEGA_e_dot*t_oe ; 
% Final sat position in earth-fixed coordinates. 

x_k = x_k_prime*cos(omega_k) - y - k_prime*cos(Lk)*sin(omega_k) ; 

y_k = x_k_prime*sin(omega_k) + y_k_prime*cos(i_k)*cos(omegaJc) ; 

z_k = y_k_prime*sin(i_k) ; 
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vec_satellite_ecf = [x_k;y__k;z_k] ; 
end 

function x = kepler(a,b,epsilon,N) 

%KEPLER KEPLER(a,b,epsilon,N) returns the iterated solution to 

% x=a*sin(x)+b. Iterates until the new value is within epsilon 

% of the old value or until the number of iterations 

% reaches N. 

old_x = 0; 

new_x = b; 

iterations=0; 

while ((abs(new_x-old_x)>epsilon) & (iterations<N)) 
old_x = new_x; 
%disp(new_x); 
new_x = a*sin(o!d_x)+b; 
iterations = iterations+1; 

end 

if iterations <N 

x=new_x; 

%dispCIterations:'); 
%disp(iterations); 
%disp(a); 
%disp(b); 
%disp(x); 
elseif iterations==N 

disp(Tixed Point Iteration Failed'); 

end 



function [param_vec] = sv7_eph0 

EphemerisSV071ength = 167;%bytes 
SV_PRN = 7; 

t = 483634.754; 

t.ephem = 483042; 

weeknum = 763; 

codeL2 = 1; 

L2Pdata = 0; 

SVacc_raw = 7; 
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SVJiealth 

IODC 

T_GD 

t_oc 

a_f2 

a_fl 

a_flO 

SVacc 

IODE 

fit_intvl 

C_rs 

della_n 

M_0 

C_uc 

ecc 

C_us 

sqrt_A 

t_oe 

C_ic 

OMEGA.O 

C_is 

LO 

C_rc 

omega 

OMEGADOT 

IDOT 

Axis 

n 

rlme2 

OMEGA_n 

ODOT.n 



= 0; 



= 0; 



120; 

1.39698e^-09; 

485504; 

0; 

1.13687e~13; 

0.000697501; 

32; 

120; 

-17.2812; 

4.39733e^09; 

-1.6207; 

-9.27597e-07; 

0.00659284; 

9.46783e-06; 

5153.78; 

485504; 

1.47149e-07; 

0.793991; 

-9.31323e-09; 

0.962958; 

197.938; 

-2.69189; 

-7.93212e-09; 

7.82175^11; 

2.65614e+07; 

0.00014585; 

0.999978; 

-34.6095; 

-7.29291e-05; 



param_vec = [t; 

M_0; 

delta_n; 

ecc; 

sqrt_A; 

OMEGA_0; 

LO; 

omega; 

OMEGADOT, 
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IDOT, 

C_uc; 

C_us; 

C_rc; 

C_rs; 

C_ic; 

C_is; 

t_oe; 

IODE]; 



function [param_vec] = sv2_ephQ 



EphemerisSV021ength 
SV.PRN 



t_ephem 

weeknum 

codeL2 

L2Pdaia 

SVacc_raw 

SV_heallh 

IODC 

T_GD 

a_f2 

a_n 

aJO 

SVacc 

IODE 

fujntvl 

C_rs 

delta_n 

M_0 

C_uc 

ecc 

C_us 

sqrt_A 

t_oe 

C_ic 

OMEGA.O 



= 167;%bytes 

= 2; 



7; 
0; 



= 0; 



483634.754; 

483042; 

763; 

l; 
0; 



165; 

9.31323e-10; 

489600; 

0; 

-2.16005e-12; 

-0.000109646; 

32; 

165; 

-30.6562; 

4.92235e^-09; 

0.0785892; 

-1.52551e-06; 

0.0134761; 

4.63426e^06; 

5153.66; 

489600; 

1.09896^-07; 

-0.26998; 
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C_is 
LO 
C_rc 
omega 

OMEGADOT 

IDOT 

Axis 

n 

rlme2 

OMEGA_n 

ODOT_n 

param_vec = [t; 



= 1.63913&-07; 
= 0.953377; 
= 285.688; 
= -2.63668; 
= -8.54964e-09; 
= -2.16795e-10; 
= 2.65602e+07; 
= 0.00014586; 
= 0.999909; 
= -35.9722; 
= -7.29297e-05; 



MJ>; 

delta_n; 

ecc; 

sqrt_A; 

OMEGA_0; 

LO; 

omega; 

OMEGADOT; 

IDOT; 

C_uc; 

C_us; 

C_rc; 

C_rs; 

C_ic; 

C_is; 

t_oe; 

IODE]; 



function [param_vec] = sv9_eph0 

EphemerisSV091ength = 167;%bytes 
SV PRN = 9; 



t_ephem 

weeknum 

codeL2 

L2Pdata 



= 483634.754; 

= 483042; 

= 763; 
= 1; 
= 0; 
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SVacc_jaw 

SVJiealih 

IODC 

T_GD 

t_oc 

a_f2 

a_fl 

aJO 

SVacc 

IODE 

fitjntvl 

C_rs 

delta_n 

M_0 

C_uc 

ecc 

C_us 

sqrt_A 

t_oe 

CJc 

OMEGA_0 

C_is 

i_0 

C_rc 

omega 

OMEGADOT 

IDOT 

Axis 

n 

rlme2 

OMEGA_n 

ODOT.n 



7; 
0; 



= 0; 



592; 

1.39698e-09; 

489600; 

0; 

-1.13687e-12; 

-5.16325O-06; 

32; 

80; 

80.1875; 

4.85127e-09; 

-2.65977; 

4.25801e-06; 

0.00292443; 

5.34952e^-06; 

5153.73; 

489600; 

5.58794^-09; 

-1.28659; 

2.235 17e-08; 

0.950485; 
: 270.656; 
= -0.581175; 
: -8.33535e^-09; 
; 2.4501e-10; 
= 2.6561e+07; 
: 0.000145854; 
: 0.999996; 
= -36.9888; 
: -7.29295e-05; 



param_vec = [t; 

MJO; 

delta_n; 

ecc; 

sqrt_A; 

OMEGA J); 

L0; 

omega; 
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OMEGADOT, 

IDOT; 

C_uc; 

C_us; 

Cjcc; 

C_rs; 

C_ic; 

C_is; 

t_oe; 

IODE]; 



function [param_vec] = sv4_ephQ 



EphemerisS V041ength 
SV.PRN 



t_ephem 

weeknum 

codeL2 

L2Pdata 

SVacc_raw 

SV_healih 

IODC 

T_GD 

t_oc 

a_f2 

a_n 

a_ffi 

SVacc 

IODE 

fit_intvl 

C_rs 

delta_n 

M_0 

C_uc 

ecc 

C_us 

sqrt_A 

t_oe 

C ic 



= 167;%bytes 

= 4; 



7; 
0; 



= 0; 



483634.754; 

483048; 

763; 

l; 

0; 



711; 

1. 39698&-09; 

489600; 

0; 

1.59162e-12; 

4.03658e-05; 

32; 

199; 

15.4375; 

4.67448e-09; 

2.57307; 

9.76026e^-07; 

0.00303051; 

6.10203e-06; 

5153.62; 

489600; 

4.09782e-08; 
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OMEGA_0 

C_is 

LO 

C.rc 

omega 

OMEGADOT 

IDOT 

Axis 

n 

rlme2 
OMEGA_n 
ODOT n 



= 1.86007; 
= 1.11759e-08; 
= 0.963886; 
= 261.562; 
= -1.24408; 
= -8.10534e-09; 
= 2.625 lle-10; 
= 2.65598e+07; 
= 0.000145863; 
= 0.999995; 
= -33.8421; 
= -7.29293e-05; 



param_vec = [t; 



function [param_vec] = sv!2_eph0 



M_0; 

delta_n; 

ecc; 

sqrt_A; 

OMEGA_0; 

L0; 

omega; 

OMEGADOT; 

IDOT; 

C_uc; 

C_us; 

C_rc; 

C_rs; 

C_ic; 

CJs; 

t_oe; 

IODE]; 



% This ephemeris data was extracted from test0826.asc 
% by Glen Brooksby. 

EphemerisSV121ength = 167;%bytes 
SV.PRN = 12; 



= 483634.754; 



t_ephem 



= 482748; 
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weeknum 

codeL2 

L2Pdata 

SVacc_raw 

SVJiealth 

IODC 

T_GD 

t_oc 

a_£2 

a_fl 

a_fO 

SVacc 

IODE 

fit_intvl 

C_rs 

delta_n 

m_0 

C_uc 

ecc 

C_us 

sqrt_a 

t_oe 

C_ic 

omega_0 

C_is 

LO 

C_rc 

omega 

OMEGADOT 

IDOT 

Axis 

n 

rlme2 

OMEGA_n 

ODOT_n 



param_vec = [t; 



= 763; 
= l; 
= 0; 

2; 
0; 

= 323; 

= 2.79397&-09; 
= 489600; 
= 0; 

= -1.21645e-ll; 
= -2.44565C-05; 
= 4; 
= 67; 

0; 

= 42.75; 
= 1.96937&-09; 
= 3.01393; 
= 2.28174e-06; 
= 0.0147704; 
= 5.3402^-06; 
= 5153.5; 
= 489600; 
= -1.65775e-07; 
= -0.925903; 
= 3.91155e-08; 
= 1.08737; 
= 338.719; 
= -0.167314; 
= -6.63885e-09; 
= L2572e~10; 
= 2.65585e+07; 
= 0.000145871; 
= 0.999891; 
= -36.6281; 
= -7.29278e-05; 



m_0; 
delta_n; 
ecc; 
sqrt_a; 
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omega_0; 

LO; 

omega; 

OMEGADOT; 

IDOT; 

C_uc; 

C_us; 

C_rc; 

C_rs; 

CJc; 

C.is; 

t_oe; 

IODE]; 



